#include "vs_rea_xx.hpp"
#include <iomanip>

#ifndef VS_REA_XX_USE_CONFIG_H
#define VS_REA_XX_NAME ("vs_rea_xx")

#define VS_REA_XX_RS485_PATH ("/dev/ttyUSB1")
#define VS_REA_XX_RS485_BAUDRATE (921600)
#define VS_REA_XX_RS485_DE_PATH ("/dev/gpiochip3")
#define VS_REA_XX_RS485_DE_PIN (24)
#define VS_REA_XX_RS485_FRAME_TIMEOUT_MS (5)

#define VS_REA_XX_RS485_RECV_LEN (22)
#endif // VS_REA_XX_USE_CONFIG_H

uart_v2 *Pvs_rea_xx_uart = NULL;
rs485_v3 *Pvs_rea_xx_rs485 = NULL;

static yj_return init_flag = YJ_ERROR;

int vs_rea_xx_rs485_read(uint8_t *data, uint32_t len)
{
    if (!Pvs_rea_xx_rs485 || !Pvs_rea_xx_uart || !data)
        return -1;
    return Pvs_rea_xx_rs485->rs485_recv_with_length(data, len);
}

int vs_rea_xx_rs485_write(uint8_t *data, uint32_t len)
{
    if (!Pvs_rea_xx_rs485 || !Pvs_rea_xx_uart || !data)
        return -1;
    return Pvs_rea_xx_rs485->rs485_send(data, len);
}

yj_return vs_rea_xx_init(void)
{
    std::string name(VS_REA_XX_NAME);
    Pvs_rea_xx_uart = new uart_v2(VS_REA_XX_RS485_PATH, VS_REA_XX_RS485_BAUDRATE, 8, 1);
    // Pvs_rea_xx_rs485 = new rs485_v3(Pvs_rea_xx_uart, VS_REA_XX_RS485_DE_PATH, VS_REA_XX_RS485_DE_PIN, VS_REA_XX_RS485_FRAME_TIMEOUT_MS);
    Pvs_rea_xx_rs485 = new rs485_v3(Pvs_rea_xx_uart, VS_REA_XX_RS485_FRAME_TIMEOUT_MS);

    if (Pvs_rea_xx_rs485->rs485_open() != YJ_OK)
        return init_flag;

    init_flag = YJ_OK;
    return init_flag;
}

void vs_rea_xx_deinit(void)
{
    if (Pvs_rea_xx_rs485)
    {
        Pvs_rea_xx_rs485->rs485_close();
        delete Pvs_rea_xx_rs485;
        Pvs_rea_xx_rs485 = NULL;
    }
    if (Pvs_rea_xx_uart)
    {
        delete Pvs_rea_xx_uart;
        Pvs_rea_xx_uart = NULL;
    }
}
yj_return v_rea_xx_count_checksum(uint8_t *data)
{
    if (!data)
    {
        LOG(ERROR) << "❌ v_rea_xx_count_checksum data is null";
        return YJ_ERROR;
    }
    uint8_t checksum = 0;
    for (int i = 0; i < 21; i++)
    {
        checksum ^= data[i];
    }
    if (checksum != data[21])
    {
        LOG(ERROR) << "❌checksum error";
        return YJ_ERROR;
    }
    // LOG(INFO) << "✅checksum ok";
    return YJ_OK;
}

yj_return vs_rea_xx_correct_data_stream(uint8_t *data)
{
    uint32_t try_times = 220;
    uint8_t data_swap[VS_REA_XX_RS485_RECV_LEN] = {0};
    if (!data)
    {
        LOG(ERROR) << "❌ vs_rea_xx_correct_data_steam data is null";
        return YJ_ERROR;
    }

    if (data[0] != 0xAB || data[1] != 0xA0)
    {
        while ((data_swap[0] != 0xAB || data_swap[1] != 0xA0) && try_times--)
        {
            vs_rea_xx_rs485_read(data_swap, 2);
        }

        if (try_times == 0)
        {
            LOG(ERROR) << "❌ vs_rea_xx_correct_data_steam try_times is 0";
            return YJ_ERROR;
        }
        vs_rea_xx_rs485_read(data_swap + 2, VS_REA_XX_RS485_RECV_LEN - 2);
        memcpy(data, data_swap, VS_REA_XX_RS485_RECV_LEN);
    }

    return YJ_OK;
}

yj_return vs_rea_xx_read_all(vs_rea_xx_data_t *data)
{
    uint32_t temp1, temp2, temp3;
    uint8_t read_buff[VS_REA_XX_RS485_RECV_LEN] = {0};
    vs_rea_xx_data_t out_buff = {0};
    if (init_flag == YJ_ERROR || !data)
    {
        LOG(ERROR) << "❌ vs_rea_xx_read_all init_flag error or data is null";

        return YJ_ERROR;
    }

    if (vs_rea_xx_rs485_read(read_buff, VS_REA_XX_RS485_RECV_LEN) == 0)
    {
        // LOG(ERROR) << "❌ vs_rea_xx_rs485_read nothing";
        return YJ_NULL;
    }

    if (vs_rea_xx_correct_data_stream(read_buff) == YJ_ERROR)
    {
        LOG(ERROR) << "❌ vs_rea_xx_correct_data_steam error";
        return YJ_ERROR;
    }

    if (v_rea_xx_count_checksum(read_buff) == YJ_ERROR)
    {
        return YJ_ERROR;
    }

    temp1 = read_buff[9] | (read_buff[10] << 8) | (read_buff[11] << 16) | (read_buff[12] << 24);
    out_buff.X_accel = *(float *)((void *)(&temp1)); // m/s^2
    temp2 = read_buff[13] | (read_buff[14] << 8) | (read_buff[15] << 16) | (read_buff[16] << 24);
    out_buff.Y_accel = *(float *)((void *)(&temp2)); // m/s^2
    temp3 = read_buff[17] | (read_buff[18] << 8) | (read_buff[19] << 16) | (read_buff[20] << 24);
    out_buff.Z_accel = *(float *)((void *)(&temp3)); // m/s^2

    // {
    //     for (int i = 0; i < 22; i++)
    //     {
    //         LOG(INFO) << "read_buff[" << i << "] = " << std::hex << std::setw(2) << std::setfill('0') << std::uppercase << static_cast<int>(read_buff[i]);
    //     }
    //     LOG(INFO) << "X_accel = " << out_buff.X_accel;
    //     LOG(INFO) << "Y_accel = " << out_buff.Y_accel;
    //     LOG(INFO) << "Z_accel = " << out_buff.Z_accel;
    //     LOG(INFO) << "temp1 = "<< std::hex << std::setw(2) << std::setfill('0') << std::uppercase << temp1;
    //     LOG(INFO) << "temp2 = "<< std::hex << std::setw(2) << std::setfill('0') << std::uppercase << temp2;
    //     LOG(INFO) << "temp3 = "<< std::hex << std::setw(2) << std::setfill('0') << std::uppercase << temp3;
    // }

    data->X_accel = out_buff.X_accel;
    data->Y_accel = out_buff.Y_accel;
    data->Z_accel = out_buff.Z_accel;

    return YJ_OK;
}

void vs_rea_xx_print_data(vs_rea_xx_data_t *data)
{
    if (!data)
    {
        LOG(ERROR) << "vs_rea_xx_print_data data is null";
        return;
    }

    LOG(INFO) << "\n============= data start =============\n"
              << "X_accel: " << data->X_accel << "m/s^2\n"
              << "Y_accel: " << data->Y_accel << "m/s^2\n"
              << "Z_accel: " << data->Z_accel << "m/s^2\n"
              << "============= data end =============\n";
}